﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;

namespace Flight
{
    public class AirplaneAutopilot : IAutopilot
    {
        public bool
            ap_alt,
            ap_speed,
            ap_heading;
        private float
            _target_vspeed,
            _target_alt,
            _target_speed,
            _target_heading;

        public AirplaneAutopilot()
        {
            ap_alt = ap_speed = ap_heading = false;

            targetVSpeed = 500f / 60;
            targetAlt = 1000;
            targetSpeed = 80;
            targetHeading = 0;
        }

        public float targetVSpeed
        {
            get { return _target_vspeed; }
            set { _target_vspeed = value; }
        }
        public float targetSpeed
        {
            get { return _target_speed; }
            set
            {
                if (value < 0)
                    _target_speed = 0;
                else _target_speed = value;
            }
        }
        public float targetAlt
        {
            get { return _target_alt; }
            set
            {
                if (value < 0)
                    value = 0;
                else _target_alt = value;
            }
        }
        public float targetHeading
        {
            get { return _target_heading; }
            set
            {
                value %= 360;
                if (value < 0)
                    value += 360;
                _target_heading = value;
            }
        }

        public void update(float dt, Aircraft aircraft, ControlInputs inputs)
        {
        }

        float prevSpeedDiff = 0f;

        public ControlInputs getControlInputs(Aircraft aircraft, ControlInputs inputs)
        {
            if (!(aircraft is Airplane)) return inputs;
            Airplane airplane = (Airplane)aircraft;

            if (ap_speed)
            {
                float diff = targetSpeed - airplane.getVelocity();
                float accel = diff - prevSpeedDiff;
                inputs.power = MathHelper.Clamp(diff / 5 - accel / 10 + 0.3f, 0, 1);
                prevSpeedDiff = diff;
            }
            if (ap_heading)
            {
                float diff = targetHeading - airplane.getHeading();
                diff %= 360;
                Console.WriteLine("diff = " + diff);
                if (diff > 180)
                    diff -= 360;
                else if (diff < -180)
                    diff += 360;
                diff = -diff;
                Console.WriteLine("diff = " + diff);
                inputs.yaw = MathHelper.Clamp((diff - airplane.getYaw() * 2) / 100, -1, 1);
                Console.WriteLine("yaw = " + inputs.yaw + "\n");
            }
            if (ap_alt)
            {
                float diff = targetAlt - airplane.getLoc().Z;
                float alt_hold_diff = targetVSpeed * targetVSpeed * 0.36f; // vs*3=alt_hold_diff*vs^2 @ vs=500 ft/min = 500/60 ft/s
                if (Math.Abs(diff) > alt_hold_diff)
                {
                    float signed_vspeed = diff > 0 ? targetVSpeed : -targetVSpeed;
                    float vspeed_diff = signed_vspeed - airplane.getVSpeed();
                    inputs.pitch = MathHelper.Clamp(vspeed_diff / 10, -1, 1);
                }
                else
                {
                    inputs.pitch = MathHelper.Clamp((diff - airplane.getVSpeed() * 3) / 100, -1, 1);
                }
            }

            return inputs;
        }
    }
}
